#include "include/data_subpub.h"

// 构造函数实现
// DataSubscriber::DataSubscriber(
//     const rclcpp::Node::SharedPtr& node,
//     const std::shared_ptr<DataHandler>& handler
// ) : node_(node), handler_(handler)
// {
//     // stateManager_ = &StateConfigManager::getInstance(); // 在构造函数体内赋
    
//     LOG_CRITICAL("DATA SUBPUSCRIBE INIT SUCESSS!");

// }

DataSubscriber& DataSubscriber::getInstance() {
    // 静态局部变量，确保全局唯一
    static DataSubscriber instance;
    return instance;
}


void DataSubscriber::onInit(const rclcpp::Node::SharedPtr& node, const std::shared_ptr<DataHandler>& handler)
{
    node_ = node;
    handler_ = handler;
    Init();

}
// DataSubscriber& DataSubscriber::getInstance(
//     const rclcpp::Node::SharedPtr& node,
//     const std::shared_ptr<DataHandler>& handler
// ) {
//     // 静态局部变量，确保全局唯一
//     static DataSubscriber instance(node, handler);
//     return instance;
// }



void DataSubscriber::Init() 
{
    lidar_data_sub_ = node_->create_subscription<msg::ObstacleData>(
        SLAM_COARSE_OBSTACLE_DATA,
        1,                          
        [this](const msg::ObstacleData::SharedPtr msg) {
            if (handler_) {
                handler_->handleData(msg, DataType::Lidar);
            }
            else
            {
                std::cout << "err: " << std::endl;
            }
        }
    );


    ai_data_sub_ = node_->create_subscription<msg::AiMetadata>(
        DATA_AIMETA, 
        1, 
        [this](const msg::AiMetadata::SharedPtr msg) {
        if (handler_) {
                handler_->handleData(msg, DataType::AI);
            }
        }
    );

    app_data_sub_ = node_->create_subscription<msg::AppCamdar>(
        APP2CAMDAR_STATUS, 
        1, 
        [this](const msg::AppCamdar::SharedPtr msg){
        if (handler_) {
                handler_->handleData(msg, DataType::APP); 
            }
        }
    );

    camdar_status_pub_ = node_->create_publisher<msg::FusionStatus>(CAMDAR_FUSION_STATUS, 1);
    // 设置一个定时器来定期发布消息
    timer_ = node_->create_wall_timer(
        std::chrono::milliseconds(2000),  // 每100毫秒发布一次
        std::bind(&DataSubscriber::publishCamdarStatus, this)  // 修正回调函数名称
    );

}


void DataSubscriber::publishCamdarData(const std::shared_ptr<msg::FusionCamdar> &msg)
{
    camdar_data_pub_->publish(*msg);

}


void DataSubscriber::publishCamdarStatus()
{
    int sensor = 0;
    int status = 0;
    int ai_status = 0;
    int li_status = 0;
    long long ai_pre_time = 0;
    long long li_pre_time = 0;

    bool bSend = gscm.sendStatus(sensor, status, ai_status,li_status, ai_pre_time, li_pre_time);
    if(bSend)
    {
        auto msg = std::make_shared<msg::FusionStatus>();
        msg->status = status;
        msg->ai_status = ai_status;
        msg->li_status = li_status;
        msg->ai_time_stamp = ai_pre_time;
        msg->li_time_stamp = li_pre_time;
        camdar_status_pub_->publish(*msg);
        LOG_CRITICAL("Sensor  STATUS:{} {} {} {} {} {}", sensor, status, ai_status, li_status, ai_pre_time, li_pre_time);
    }
}

